import socket
import time
# 096:093:094:093:100:090
import pid
import numpy as np
import opencolor
import math
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
# 使用套件字收发数据
# 目标IP 和端口，元组类型
ip_adders = ('192.168.137.2', 8008)
flag = False


def send(send_data):
    # 创建一个udp套件字
    udp_socket.sendto(send_data.encode('utf-8'), ip_adders)
    # 关闭套件字
    time.sleep(0.01)


def close_socket():
    udp_socket.close()


def accept():
    # 创建一个udp套件字
    udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    # 绑定本地相关信息，如果不绑定，则系统会随机分配，必须绑定本电脑的ip和port
    local_addr = ('192.168.137.1', 8010)  # 元组的第一个参数为本机IP，可以为空字符串，会自动生成本机IP
    udp_socket.bind(local_addr)
    # 等待接收方发送数据
    # rs中存储的是一个元组（接收到的数据，（发送方的ip，port））
    rs_data = udp_socket.recvfrom(1024)
    rs_masg = rs_data[0]
    # rs_addr = rs_data[1]
    # 接收到的数据解码展示
    # print(rs_masg.decode('utf-8'))
    return rs_masg.decode('utf-8')

    # 关闭套件字
    udp_socket.close()


def get_yaw(rs_msg):
    cont = 0
    yaw = ''
    for s in range(len(rs_msg)):
        if rs_msg[s] == ":":
            cont += 1
        if cont == 2:
            s += 1
            while rs_msg[s] != ":":
                yaw += rs_msg[s]
                s += 1
            return yaw


def map(data):
    """
    归一化映射到任意区间
    :param data: 数据
    :param MIN: 目标数据最小值
    :param MAX: 目标数据最小值
    :return:
    """
    if data > 0:
        MAX = 1650
        MIN = 1550
        return (data / 180) * (MAX - MIN) + MIN
    if data < 0:
        MAX = 1500
        MIN = 1350
        return (data / -180) * (MAX - MIN) + MIN
        d_min = np.max(data)    # 当前数据最大值
        d_max = np.min(data)    # 当前数据最小值
    return MIN + (MAX - MIN) / (d_max - d_min) * (data - d_min)


if __name__ == '__main__':
    send_data = "1520:1520:1520:1520:100:090"
    left = 1520
    right = 1520
    rs_data = accept()  # 接收数据
    begin_yaw = get_yaw(rs_data)  # 起始罗盘位置
    print("起始位置：" + begin_yaw)
    # "0"+left+":"+"0"+right+":094:093:100:090"
    send_data = str(left) + ":" + str(right) + ":1520:1520:100:090"
    #go_data= "0" + str(left) + ":" + "0" + str(right) + ":094:093:100:090"
    print(send_data)
    # 0-180  1520-1650
    # -180 -0  1350-1500
    ins = input()
    if ins == "go":
        flag = True
    pid = pid.PID(0.1, 180, -180, 0.1, 0.01, 0.005)
# 扭转后 按下start键
    i = 0
    while flag:
        i += 1
        send(send_data)  # 发送控制数据
        rs_data = accept()  # 接收数据
        now_yaw = int(get_yaw(rs_data))  # 起始罗盘位置
        print("现在位置：" + str(now_yaw))
        val = int(begin_yaw)
        inc = pid.calculate(val, now_yaw)
        left = int(map(inc))
        right = int(left)
        print("val:%f inc:%f" % (now_yaw, inc))
        print(left, " ", right)
        left_data = str(left)
        right_data = str(right)
        send_data = str(left) + ":" + str(right) + ":1520:1520:100:090"
        print(send_data)
        if 5 > int(now_yaw) - int(begin_yaw) > -5:
            cont = 0
            flag = True
            while flag:
                rs_data = accept()  # 接收数据
                now_yaw = get_yaw(rs_data)  # 起始罗盘位置
                if 10 > int(now_yaw) - int(begin_yaw) > -10:
                    flag = False
                time.sleep(0.3)
                cont += 1
                if cont > 5:
                    time.sleep(0.1)
                    # for j in cont:
                    #     send_data = "1560:1480:1520:1520:100:090"
                    opencolor.color()
